﻿/*****************************************************************//**
 * \file   SurgicalRobotCommunication.cpp
 * \brief  负责上层、算法层、底层通信类
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/
#include "./include/SurgicalRobotCommunication.h"
#include <QDebug>
#include <QImage>
#include <sys/io.h>
#include "../hardware_interface/Robot/aubo/AuboRobotControl.h"
#include "../hardware_interface/Robot/UR3/UR3Control.h"

#include "../hardware_interface/EndTool/camera/EndToolCamera.h"
#include "../hardware_interface/EndTool/3dof/EndTool3Dof.h"
#include "../hardware_interface/EndTool/4dof/EndTool4Dof.h"
#include "../hardware_interface/EndTool/BoplarHook/EndToolBoplarHook.h"
#include "../hardware_interface/EndTool/forcep/EndToolForcep.h"

#include <pthread.h>
#include <sched.h>
#include <unistd.h>


 //机械臂配置数据
SRobotConfig sRobotConfigPara[MaxRobotNum];

//机械臂运行时数据
SRobotRealTimeData sRobotRunData[MaxRobotNum];

//力控交互设备
SRobotHapticDeviceData sHapticRunData[2];

//私有静态类成员变量在类外初始化
SurgicalRobotCommunication* SurgicalRobotCommunication::m_pInstance = NULL;

QImage qComImage;
//信号量的handle
sem_t hHapticControlRobotSemaphore;
sem_t hHapticSemaphore;
sem_t hReadRobotDataSemaphore;
sem_t hCutfusion;
sem_t hHapticForceFeedback;
sem_t hHapticreadSemaphore;

unsigned int iCamera = 0;
unsigned int iGet = 0;
unsigned int iSurgical = 0;
unsigned int iControl = 0;
unsigned int iReader = 0;
/**
 * \brief   SurgicalRobotCommunication类的构造函数.
 *          初始化系统运行时数据和配置参数数据变量
 */
SurgicalRobotCommunication::SurgicalRobotCommunication()
{
	memset(&sRobotConfigPara, 0, sizeof(sRobotConfigPara));
	memset(&sRobotRunData, 0, sizeof(sRobotRunData));
	memset(&sHapticRunData, 0, sizeof(sHapticRunData));

	for (int i = 0; i < MaxRobotNum; i++)
	{
		robotControl[i] = nullptr;
		endTool[i] = nullptr;
	}

#if DEBUG_ROBOT_READ
	read_robot_experimrnt.open("C:\\20210812\\SurgicalRobotPlatform\\SurgicalRobotPlatform\\experiment_data\\read.txt", std::ios::out);
#endif

}

/**
 * \brief   SurgicalRobotCommunication类的析构函数.
 *          删除之前创建的对象，释放内存空间
 *
 */
SurgicalRobotCommunication::~SurgicalRobotCommunication()
{
    for (int i = 0; i < MaxRobotNum; i++)
    {
        if (endTool[i])
        {
            delete endTool[i];
            endTool[i] = nullptr;
        }
        if (robotControl[i])
        {
            delete robotControl[i];
            robotControl[i] = nullptr;
        }
    }


	if (pGeneraAlgirithm)
	{
		delete pGeneraAlgirithm;
		pGeneraAlgirithm = nullptr;
	}


}

/**
 * \brief   初始化对应的机械臂.
 *
 * \param robotNumber  表示第几个机械臂
 * \param ipAddr       机械臂的IP地址
 * \return             true 表示成功，false表示失败
 */
bool SurgicalRobotCommunication::initRobot(int robotNumber, const char* ipAddr)
{
	int robotType = 0;

	if (robotNumber >= MaxRobotNum) {
		qDebug() << "Please check you robot number " << robotNumber;
		return false;
	}

	robotType = sRobotConfigPara[robotNumber].iRobotarmType;

	switch (robotType) {
	case ARMTYPE_AOBOi5:
        robotControl[robotNumber] = new AuboRobotControl();
		if (robotControl[robotNumber]->robotLogin(ipAddr, AuboRobotNetPort))
		{
			robotControl[robotNumber]->bInitDone = true;
		}
		else {
			robotControl[robotNumber]->bInitDone = false;
		}
		break;
    // case ARMTYPE_FRANKA:
    // 	robotControl[robotNumber] = new FrankaRobotControl();
    // 	if (robotControl[robotNumber]->robotLogin(ipAddr, FrankaRobotNetPort))
    // 	{
    // 		robotControl[robotNumber]->bInitDone = true;
    // 	}
    // 	else {
    // 		robotControl[robotNumber]->bInitDone = false;
    // 	}
    // 	break;
	case ARMTYPE_UR3:
		robotControl[robotNumber] = new UR3Control();
		if (robotControl[robotNumber]->robotLogin(ipAddr, UR3RobotNetPort))
		{
			robotControl[robotNumber]->bInitDone = true;
		}
		else {
			robotControl[robotNumber]->bInitDone = false;
		}
		break;
	default:
		qDebug() << "Please check you robot type " << robotType;
		robotControl[robotNumber]->bInitDone = false;
		break;
	}
	return robotControl[robotNumber]->bInitDone;
}

/**
 * \brief   末端器械初始化.
 *          根据机械臂对应的末端器械，初始化对应的末端器械
 * \param robotNumber 机械臂编号
 * \return
 */
bool SurgicalRobotCommunication::initEndTool(int robotNumber)
{
	int toolType = 0;

	if (robotNumber >= MaxRobotNum) {
		qDebug() << "Please check you robot number " << robotNumber;
		return false;
	}

	toolType = sRobotConfigPara[robotNumber].iRobotToolType;
	qDebug() << "toolType:" << toolType << "robotNumber:" << robotNumber;
	switch (toolType) {
	case TOOLTYPE_CAMERA:
		endTool[robotNumber] = new EndToolCamera();
		break;
	case TOOLTYPE_BoplarHook:
		endTool[robotNumber] = new EndToolBoplarHook();
		break;
	case TOOLTYPE_4DOF:
		endTool[robotNumber] = new EndTool4Dof();
		break;
	case TOOLTYPE_3DOF:
		endTool[robotNumber] = new EndTool3Dof();
		break;
	case TOOLTYPE_FORCEP:
		endTool[robotNumber] = new EndToolForcep();
		break;
	default:
		return false;
	}

	endTool[robotNumber]->bEndToolInitDone = true;
	qDebug() << endTool[robotNumber]->getEndToolType();
	return true;
}
/**
 * \brief  初始化机械臂配置参数 .
 *         如果之前有配置过机械臂参数，则用之前配置过的，否则用默认的
 */
void SurgicalRobotCommunication::InitConfigParam()
{
	std::string robotConfigFile = paramsPath + robotArmConfigFile;

	if (!access(robotConfigFile.c_str(), 0))
	{
		ReadConfigParam();
		return;
	}
	else {
		for (int i = 0; i < 4; i++)
		{
			strcpy(sRobotConfigPara[i].cRobotArmIP, "192.168.65.3");
			strcpy(sRobotConfigPara[i].cRobotArmMA, "255.255.255.0");
			strcpy(sRobotConfigPara[i].cRobotArmGW, "192.168.65.1");
			//RCM点定位
			strcpy(sRobotConfigPara[i].cCoordinateSystem, "X,Y,Z");
			//RCM点校准
			sRobotConfigPara[i].dRCMPosition[0] = 0.01;
			sRobotConfigPara[i].dRCMPosition[1] = 0.01;
			sRobotConfigPara[i].dRCMPosition[2] = 0.01;
		}

		//设定机械臂型号初值
		sRobotConfigPara[0].iRobotarmType = ARMTYPE_AOBOi5;
		sRobotConfigPara[1].iRobotarmType = ARMTYPE_AOBOi5;
		sRobotConfigPara[2].iRobotarmType = ARMTYPE_AOBOi5;
		sRobotConfigPara[3].iRobotarmType = ARMTYPE_FRANKA;

		//设定器械型号初值
		sRobotConfigPara[0].iRobotToolType = TOOLTYPE_BoplarHook;
		sRobotConfigPara[1].iRobotToolType = TOOLTYPE_4DOF;
		sRobotConfigPara[2].iRobotToolType = TOOLTYPE_4DOF;
		sRobotConfigPara[3].iRobotToolType = TOOLTYPE_CAMERA;
		HDControl[0].Haptictype = HAPTICTYPE_LEFT;
	}

	return;
}

/**
 * \brief   保存配置参数.
 *
 */
bool SurgicalRobotCommunication::SaveRobotConfig()
{
    FILE* fp = NULL;
    std::string robotConfigFile = paramsPath + robotArmConfigFile;

    if(access(paramsPath.c_str(), 0)){
        qDebug() << "save robot config path " << paramsPath.c_str() << "is not exist!";
        return false;
    }

    fp = fopen(robotConfigFile.c_str(), "wb+");
    if (fp == nullptr)
    {
        qDebug() << "open file " << robotConfigFile.c_str() << "filed!";
        return false;
    }
    for (int i = 0; i < MaxRobotNum; i++)
    {
        fwrite(&sRobotConfigPara[i], sizeof(sRobotConfigPara[i]), 1, fp);
    }

    fclose(fp);

    return true;

}


/**
 * \brief   读取机械臂与器械的配置参数.
 *
 */
void SurgicalRobotCommunication::ReadConfigParam()
{
	FILE* fp = nullptr;
	std::string robotConfigFile = paramsPath + robotArmConfigFile;

    if (access(robotConfigFile.c_str(), 0)) {
		qDebug() << "read robot config path " << robotConfigFile.c_str() << "is not exist!";
		return;
	}

	fp = fopen(robotConfigFile.c_str(), "r");
	if (fp == nullptr)
	{
		qDebug() << "open robot config file " << robotConfigFile.c_str() << "failed!";
		return;
	}

	for (int i = 0; i < MaxRobotNum; i++)
	{
		fread(&sRobotConfigPara[i], sizeof(sRobotConfigPara[i]), 1, fp);
	}

	fclose(fp);

	return;
}


/**
 * \brief   读取机械臂运行状态数据和末端器械状态数据.
 *
 */

// void setThreadAffinity(int cpu) {
//     cpu_set_t cpuset;
//     CPU_ZERO(&cpuset);
//     CPU_SET(cpu, &cpuset);

//     pthread_t currentThread = pthread_self();  // 获取当前线程

//     int result = pthread_setaffinity_np(currentThread, sizeof(cpu_set_t), &cpuset);
//     if (result != 0) {
//         perror("pthread_setaffinity_np");
//     }
// }

void SurgicalRobotCommunication::setThreadAffinity(int cpu) {
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(cpu, &cpuset);

    // 获取当前线程的句柄
    pthread_t currentThread = pthread_self();

    // 设置线程的 CPU 亲和性
    int result = pthread_setaffinity_np(currentThread, sizeof(cpu_set_t), &cpuset);
    if (result != 0) {
        qWarning() << "pthread_setaffinity_np failed:" << strerror(result);
    } else {
        qDebug() << "Thread affinity set to CPU" << cpu;
    }
}

void SurgicalRobotCommunication::run_code()
{
    //SetThreadAffinityMask(GetCurrentThread(), 2);
    setThreadAffinity(1);
	while (!bAppExit)
	{
		//WaitForSingleObject(hReadRobotDataSemaphore, INFINITE);
		iGet++;
		for (int i = 0; i < MaxRobotNum; i++)
		{
			//实时读取机械臂的网络连接状态，以及机械臂的实时关节角数据
			if ((nullptr != robotControl[i]) && robotControl[i]->bInitDone)
			{
				robotControl[i]->GetRealtimeState(sRobotRunData[i]);
			}
#if DEBUG_ROBOT_READ
			if (iGet == 100) {
				iGet = 0;
				read_robot_experimrnt << iGet << "\t" << sRobotRunData[0].sRobotDy.m4dRobotEnd.block(0, 0, 3, 1) << "\t";
			}
#endif
			//读取末端器械状态
			if (nullptr != endTool[i] && endTool[i]->bEndToolInitDone)
			{
				sRobotRunData[i].sMotorDevice.m4dToolEnd = endTool[i]->geToolEndMatrix(sRobotRunData[i].sRobotDy.m4dRobotEnd);
				sRobotRunData[i].sRobotDy.v3dToolPosture = sRobotRunData[i].sMotorDevice.m4dToolEnd.block(0, 3, 3, 1);
				sRobotRunData[i].sRobotDy.dToolPx = sRobotRunData[i].sMotorDevice.m4dToolEnd(0, 3);
				sRobotRunData[i].sRobotDy.dToolPy = sRobotRunData[i].sMotorDevice.m4dToolEnd(1, 3);
				sRobotRunData[i].sRobotDy.dToolPz = sRobotRunData[i].sMotorDevice.m4dToolEnd(2, 3);
				//getEndToolMatrix(i);
			}
		}
	}
}

/**
 * \brief   读取末端器械状态.
 *          通过机械臂获取对应末端器械的Matrix
 * \param robotNumber 第几个机械臂
 */
void SurgicalRobotCommunication::getEndToolMatrix(int robotNumber)
{
	sRobotRunData[robotNumber].sRobotDy.v3dToolPosture = sRobotRunData[robotNumber].sMotorDevice.m4dToolEnd.block(0, 3, 3, 1);
	sRobotRunData[robotNumber].sRobotDy.dToolPx = sRobotRunData[robotNumber].sMotorDevice.m4dToolEnd(0, 3);
	sRobotRunData[robotNumber].sRobotDy.dToolPy = sRobotRunData[robotNumber].sMotorDevice.m4dToolEnd(1, 3);
	sRobotRunData[robotNumber].sRobotDy.dToolPz = sRobotRunData[robotNumber].sMotorDevice.m4dToolEnd(2, 3);

	return;
}
